/* 
 * File:   surfalign_grdi3d.cc
 * Author: boyan
 * 
 * Created on April 9, 2009, 4:31 PM
 */

#include "grid3d.h"


#include <vector>
#include <set>
#include <list>

#include <Matrix3D.hpp>
#include <MsqError.hpp>
#include <VtkTypeInfo.hpp>

using Mesquite::Matrix3D;

#include "errlog.h"
#include "SurfAlign_grid3d.h"
#include "cxx_misc.h"
#include "LocalSurfaceCalculator.h"

grid3d_t *SurfNode1::grid = NULL;
grid3d_t *SurfNode::grid = NULL;
grid3d_t *SurfEdge::grid = NULL;
grid3d_t *SurfFace::grid = NULL;

SurfAlign_grid3d::SurfAlign_grid3d(grid3d_t *g)
: grid(g)
{
    grid->MAXH = GridSize(grid, grid->nodes);
    inside_elems.clear();
    inside_elems.reserve(grid->NEL);
    for(int i = 0; i < grid->NEL; ++i)
        inside_elems.push_back(i);
    initialize();
}

SurfAlign_grid3d::SurfAlign_grid3d(grid3d_t *g, std::vector<LevelsetCode> &ls)
: grid(g)
{
    grid->MAXH = GridSize(grid, grid->nodes);
    inside_elems.clear();
    inside_elems.reserve(grid->NEL);
    for(int i = 0; i < grid->NEL; ++i)
    {
        elem_t &T = grid->elems[i];
        for(int j = 0; j < 4; j++)
        {
            if(ls[T.nodes[j]] == ls_inside)
            {
                inside_elems.push_back(i);
                break;
            }
        }
    }
    initialize();
}

void SurfAlign_grid3d::initialize()
{
    SurfAlign_grid3d::GridElem::grid = grid;
    gridElems.reserve(grid->NEL);
    for(int el = 0; el < grid->NEL; el++)
        gridElems.push_back(GridElem(el));

    SurfNode::grid = grid;
    SurfEdge::grid = grid;
    SurfFace::grid = grid;
    build_faces();
    build_nodes();
    build_edges();

    /*
    Vector3D t, s, n;
    {
        createAdjacency();
        for(size_t i=0; i<surf_nodes.size(); ++i)
        {
            const std::list<int> &adjn = AdjacentNodes3[i];
            PointSet ss;
            ss.Points.reserve(adjn.size());
            std::list<int>::const_iterator j;
            for(j=adjn.begin(); j!=adjn.end(); ++j)
                ss.Points.push_back(surf_nodes[*j].toVector3D());
            ss.ComputeLocalBasis(t,s,n);
        }
    }
     */

    // compute the balls centers and mean curvatures
    std::vector<SurfNode>::iterator node;
    for(node = surf_nodes.begin(); node != surf_nodes.end(); ++node)
    {
        __compute_ball(*node);
    }
    /*
        // adjust directions of normals to point out
        std::vector<Vector3D> node_normals; // average of normals of adjusted faces
        std::set<size_t> foo_nodes; // normal in neither direction points out ??
        Vector3D NN, FN;
        size_t nid;
        // set signs of normals
        signed int dir;
        signed int Nsign;
        node_normals.resize(surf_nodes.size(), Vector3D(0.0, 0.0, 0.0));
        for(size_t fid = 0; fid < surf_faces.size(); fid++)
        {
            SurfFace &F = surf_faces[fid];
            FN = F.getFaceNormal();
            for(int i = 0; i < 3; i++)
            {
                nid = F.surf_nodes(i, nodes_inv);
                SurfNode &N = surf_nodes[nid];
                NN = N.GetBallNormal();
                Nsign = N.NormalSign;
                if((NN % FN) < 1e-12)
                    dir = -1;
                else
                    dir = 1;
                node_normals[nid] += FN;
                if(N.IsNormalSignSet() && N.NormalSign != dir)
                    foo_nodes.insert(nid);
                // assert((!N.IsNormalSignSet())||N.GetNormalSign()==dir);
                N.NormalSign = dir;
            }
        }

        std::set<size_t>::iterator it = foo_nodes.begin();
        for(; it != foo_nodes.end(); it++)
        {
            nid = it.operator *();
            SurfNode &N = surf_nodes[nid];
            node_normals[nid].normalize();
            Vector3D C1, C2;
            double d1, d2;
            if(N.Kappa > 0.0)
            {
                C1 = N.toVector3D() - node_normals[nid] / N.Kappa;
                C2 = N.toVector3D() + node_normals[nid] / N.Kappa;
                d1 = dist_l2(C1.to_array(), N.BallCenter.to_array());
                d2 = dist_l2(C2.to_array(), N.BallCenter.to_array());
                if(d1 < d2)
                {
                    N.BallCenter = C1;
                    N.NormalSign = 1;
                }
                else
                {
                    N.BallCenter = C2;
                    N.NormalSign = -1;
                }
            }
            else
                N.BallCenter = N.toVector3D() - node_normals[nid];
        }
     */
}

void SurfAlign_grid3d::build_faces()
{
    int count;
    size_t idx;
    std::vector<int> foo;

    // make surface faces
    foo.clear();
    foo.resize(grid->NF, 0);
    for(idx = 0; idx < inside_elems.size(); idx++)
    {
        int Tid = inside_elems[idx];
        elem_t &T = grid->elems[Tid];
        for(int j = 0; j < 4; j++)
            ++foo[T.faces[j]];
    }
    count = 0;
    for(idx = 0; idx < foo.size(); idx++)
        if(foo.at(idx) == 1)
            ++count;
    surf_faces.clear();
    surf_faces.reserve(count);
    for(idx = 0; idx < foo.size(); idx++)
        switch(foo.at(idx)) // the number of elements containing face idx
        {
            case 1: surf_faces.push_back(SurfFace(idx));
                break;
            case 2:break;
            default:
                error_msg("%s: error making faces\n", __func__);
        }

    // decide normal signs
    for(idx = 0; idx < surf_faces.size(); idx++)
    {
        SurfFace &F = surf_faces[idx];
        int Tid = F.elems(0);
        elem_t &T = grid->elems[Tid];
        for(int i = 0; i < 4; i++)
        {
            if(T.faces[i] == F.id)
            {
                F.swap_normal = elem_face_normal_sign(grid, &T, i) < 0;
                break;
            }
        }
    }
    // now build the inverse table
    faces_inv.resize(grid->NF, -1);
    for(size_t i = 0; i < surf_faces.size(); ++i)
        faces_inv[surf_faces[i].id] = i;
}

void SurfAlign_grid3d::build_nodes()
{
    size_t idx;
    size_t count;
    std::vector<char> foo;
    const int *nodes;

    foo.resize(grid->NN, 0);
    for(idx = 0; idx < surf_faces.size(); idx++)
    {
        nodes = surf_faces[idx].nodes();
        foo[nodes[0]] = 1;
        foo[nodes[1]] = 1;
        foo[nodes[2]] = 1;
    }
    count = 0;
    for(idx = 0; idx < foo.size(); idx++)
        if(foo[idx] == 1)
            ++count;
    LocalBall BallFactory;
    surf_nodes.reserve(count);
    for(idx = 0; idx < foo.size(); idx++)
        if(foo[idx] == 1)
        {
            surf_nodes.push_back(SurfNode(idx));
            surf_nodes.back().setCalc(BallFactory);
        }
    // now build the inverse table
    nodes_inv.resize(grid->NN, -1);
    for(size_t i = 0; i < surf_nodes.size(); ++i)
        nodes_inv[surf_nodes[i].id] = i;
}

void SurfAlign_grid3d::build_edges()
{
    size_t idx;
    size_t count;
    std::vector<char> foo;
    const int *edges;

    foo.resize(grid->NE, 0);
    for(idx = 0; idx < surf_faces.size(); idx++)
    {
        edges = surf_faces[idx].edges();
        foo[edges[0]] = 1;
        foo[edges[1]] = 1;
        foo[edges[2]] = 1;
    }
    count = 0;
    for(idx = 0; idx < foo.size(); idx++)
        if(foo[idx] == 1)
            ++count;
    surf_edges.reserve(count);
    for(idx = 0; idx < foo.size(); idx++)
        if(foo[idx])
            surf_edges.push_back(SurfEdge(idx));
    // now build the inverse table
    edges_inv.resize(grid->NE, -1);
    for(size_t i = 0; i < surf_edges.size(); ++i)
        edges_inv[surf_edges[i].id] = i;
}

/*****************************************************************************/
void SurfAlign_grid3d::__compute_ball(SurfNode &X,
        const std::vector<size_t> *edges,
        const std::vector<size_t> *faces) const
{
    X.calc->AdjacentPoints.clear();
    // get attached nodes
    if(edges == NULL)
    {
        for(size_t idx = 0; idx < surf_edges.size(); ++idx)
        {
            const SurfEdge &E = surf_edges[idx];
            if(E.has_node(X.id))
            {
                Vector3D V(grid->nodes[E.other_node(X.id)].coords);
                X.calc->AdjacentPoints.push_back(V);
            }
        }
    }
    else
    {
        for(size_t idx = 0; idx < edges->size(); ++idx)
        {
            const SurfEdge &E = surf_edges[edges->at(idx)];
            if(E.has_node(X.id))
            {
                Vector3D V(grid->nodes[E.other_node(X.id)].coords);
                X.calc->AdjacentPoints.push_back(V);
            }
        }
    }
    X.calc->computeLocalSurface();

    const double dist = 0.3 * grid->MAXH;
    Vector3D pt = X.toVector3D() + dist * X.GetSurfNormal();
    if(isPointInside(pt))
        X.flipNormal();

    return;
}

/*****************************************************************************/

bool __radius_centre_of_ball(SurfNode1 &X, std::vector<Vector3D> &pts);

void SurfAlign_grid3d::__compute_ball1(SurfNode1 &X,
        const std::vector<size_t> *edges,
        const std::vector<size_t> *faces) const
{
    // get attached nodes
    std::vector<Vector3D> pts;
    if(edges == NULL)
    {
        for(size_t idx = 0; idx < surf_edges.size(); ++idx)
        {
            const SurfEdge &E = surf_edges[idx];
            if(E.has_node(X.id))
            {
                Vector3D V(grid->nodes[E.other_node(X.id)].coords);
                pts.push_back(V);
            }
        }
    }
    else
    {
        for(size_t idx = 0; idx < edges->size(); ++idx)
        {
            const SurfEdge &E = surf_edges[edges->at(idx)];
            if(E.has_node(X.id))
            {
                Vector3D V(grid->nodes[E.other_node(X.id)].coords);
                pts.push_back(V);
            }
        }
    }


    // compute radius and center of ball
    if(!__radius_centre_of_ball(X, pts))
    {
        Vector3D N(0, 0, 0);
        if(faces == NULL)
        {
            for(size_t i = 0; i < surf_faces.size(); i++)
            {
                const SurfFace &F = surf_faces[i];
                if(F.has_node(X.id))
                    N += F.getFaceNormal();
            }
        }
        else
        {
            for(size_t i = 0; i < faces->size(); i++)
            {
                const SurfFace &F = surf_faces[faces->at(i)];
                if(F.has_node(X.id))
                    N += F.getFaceNormal();
            }
        }
        N.normalize();
        X.BallCenter = X.toVector3D() - N;
        X.Kappa = 0.0;
    }
    return;
}

bool __radius_centre_of_ball(SurfNode1 &X, std::vector<Vector3D> &pts)
{
    size_t npts = pts.size();
    std::vector<Vector3D> M;
    std::vector<double> Q;
    M.resize(npts);
    Q.resize(npts);
    for(size_t ind = 0; ind < npts; ind++)
    {
        M[ind] = pts[ind] - X.toVector3D();
        Q[ind] = 0.5 * (M[ind] % (pts[ind] + X.toVector3D())); // % is inner product of two Vector3Ds
    }
    Matrix3D A(0.0);
    Vector3D R(0.0, 0.0, 0.0), N;
    for(size_t ind = 0; ind < npts; ind++)
    {
        R += Q[ind] * M[ind];
        for(int i = 0; i < 3; i++)
            for(int j = 0; j < 3; j++)
                A[i][j] += M[ind][i] * M[ind][j];
    }

    if(det(A) < 1e-12)
        return false;
        /*{
            // pts and X are in the same plane !?!  We are going with Plan B
            // find normal as the average of normals of pairs of edges and
            //    set center so that X-X.BallCenter is coliner with normal
            R=0.0;
            for(size_t ind=0; ind<npts; ind++)
            {
                size_t jnd=(ind+1)%npts;
                N=M[ind]*M[jnd]; // * is a cross product of two Vector3Ds
                if(N.length()<1e-12)
                    continue;
                N.normalize();
                if(ind>0&&(N%R)<0)
                    R-=N;
                else
                    R+=N;
            }
            R.normalize();
            X.BallCentre()=X+R;
            X.BallKappa()=0.0;
        }*/
    else
    {
        Matrix3D invA;
        inv(invA, A);
        X.BallCenter = invA*R;
        double rad = dist_l2(X.toArray(), X.BallCenter.to_array());
        X.Kappa = 1.0 / rad;
        return true;
    }
}


/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////

grid3d_t *SurfAlign_grid3d::GridElem::grid = NULL;

bool SurfAlign_grid3d::isPointInside(const Vector3D &pos) const
{
    SurfAlign_grid3d::GridElem::grid = grid;
    for(size_t el = 0; el < inside_elems.size(); el++)
    {
        const GridElem &T = gridElems[el];
        if(T.contains_point(pos))
            return true;
    }
    return false;
}

LevelsetCode SurfAlign_grid3d::
levelset_code(Mesquite::Mesh::EntityHandle entity_handle,
        const Mesquite::Vector3D &pos) const
{
    MsqError err;
    Vector3D proj, norm;
    closest_point(entity_handle, pos, proj, norm, err);
    if(dist_l2(pos.to_array(), proj.to_array()) < 1e-6)
        return ls_onsurf;

    if(isPointInside(pos))
        return ls_inside;
    else
        return ls_outside;
}

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////

void SurfAlign_grid3d::snap_to(Mesh::EntityHandle entity_handle,
        Vector3D & coordinate) const
{
    MsqError err;
    Vector3D pos = coordinate, nor;
    closest_point(entity_handle, pos, coordinate, nor, err);
}

void SurfAlign_grid3d::normal_at(Mesh::EntityHandle entity_handle,
        Vector3D & coordinate) const
{
    MsqError err;
    Vector3D pos = coordinate, close;
    closest_point(entity_handle, pos, close, coordinate, err);
}

void SurfAlign_grid3d::normal_at(const Mesh::EntityHandle* handles,
        Vector3D coordinates[], unsigned count, MsqError & err) const
{
    (void) err;
    for(unsigned ind = 0; ind < count; ind++)
        normal_at(handles[ind], coordinates[ind]);
}

void SurfAlign_grid3d::closest_point(Mesh::EntityHandle handle,
        const Vector3D& position, Vector3D& closest, Vector3D& normal,
        MsqError & err) const
{
    (void) err;
    size_t cnidx = find_closest_node(handle, position);
    const SurfNode &N = surf_nodes[cnidx];
    normal = N.GetSurfNormal(position);
    closest = N.GetProjection(position);
}

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////

size_t SurfAlign_grid3d::find_closest_node(Mesh::EntityHandle handle,
        const Vector3D position) const
{
    (void) handle;
    // Find the nearest node
    size_t MinIdx = (size_t) - 1;
    double dist, MinDist = 1e99; // large number
    const SurfNode *cn = &surf_nodes[0];
    for(size_t idx = 0; (1e-12 < MinDist) && (idx < surf_nodes.size()); idx++)
    {
        dist = dist_l2(position.to_array(), surf_nodes[idx].toArray());
        if(dist < MinDist)
        {
            MinIdx = idx;
            MinDist = dist;
            cn = &surf_nodes[idx];
        }
    }
    return MinIdx;
}

/////////////////////////////////////////////////////////////////////////////

#include <fstream>

void SurfAlign_grid3d::write_vtk(const char* filename) const
{
    std::ofstream file(filename);
    if(!file)
        return;

    // Write a header
    file << "# vtk DataFile Version 2.0\n";
    file << "Surface mesh " << filename << " .\n";
    file << "ASCII\n";
    file << "DATASET UNSTRUCTURED_GRID\n";

    // Write vertex coordinates
    file << "\nPOINTS " << surf_nodes.size() << " float\n";
    for(size_t i = 0; i < surf_nodes.size(); i++)
    {
        Vector3D pos = surf_nodes[i].toVector3D();
        file << pos.x() << ' ' << pos.y() << ' ' << pos.z() << '\n';
    }

    file << "\nCELLS " << surf_faces.size() << ' ' << 4 * surf_faces.size() << '\n';
    for(size_t i = 0; i < surf_faces.size(); i++)
        file << 3 << ' '
            << surf_faces[i].surf_nodes(0, nodes_inv) << ' '
        << surf_faces[i].surf_nodes(1, nodes_inv) << ' '
        << surf_faces[i].surf_nodes(2, nodes_inv) << '\n';

    // Write out the element types
    file << "\nCELL_TYPES " << surf_faces.size() << '\n';
    for(size_t i = 0; i < surf_faces.size(); i++)
        file << 5 << '\n';

    // Write out normals at nodes.
    file << "\nPOINT_DATA " << surf_nodes.size()
            << "\nNORMALS node_normal float\n";
    for(size_t i = 0; i < surf_nodes.size(); ++i)
    {
        Vector3D N = surf_nodes[i].GetSurfNormal();
        file << N.x() << ' ' << N.y() << ' ' << N.z() << '\n';
    }

    // Write out normals at faces.
    file << "\nCELL_DATA " << surf_faces.size()
            << "\nNORMALS face_normal float\n";
    for(size_t i = 0; i < surf_faces.size(); ++i)
    {
        Vector3D N = surf_faces[i].getFaceNormal();
        file << N.x() << ' ' << N.y() << ' ' << N.z() << '\n';
    }

    // Close the file
    file << '\n';
    file.close();

}

#include <PatchData.hpp>
#include <iostream>

void SurfAlign_grid3d::write_vtk(Mesh *mesh, const char* filename,
        bool write_codes)
{
    Mesquite::MsqPrintError err(std::cout);
    std::ofstream file(filename);
    if(!file)
        return;

    // Write a header
    file << "# vtk DataFile Version 2.0\n";
    file << "Surface mesh " << filename << " .\n";
    file << "ASCII\n";
    file << "DATASET UNSTRUCTURED_GRID\n";

    Mesquite::PatchData pd;
    pd.set_mesh(mesh);
    pd.set_domain(this);
    pd.fill_global_patch(err);
    Mesquite::MsqVertex *vert = pd.get_vertex_array(err);
    Mesquite::MsqMeshEntity *elems = pd.get_element_array(err);

    // Write vertex coordinates
    file << "\nPOINTS " << pd.num_vertices() << " float\n";
    for(size_t i = 0; i < pd.num_vertices(); i++)
    {
        Vector3D pos = vert[i];
        file << pos.x() << ' ' << pos.y() << ' ' << pos.z() << '\n';
    }

    file << "\nCELLS " << pd.num_elements() << ' ' << (elems[0].node_count() + 1) * pd.num_elements() << '\n';
    for(size_t i = 0; i < pd.num_elements(); i++)
    {
        assert(elems[0].node_count() == elems[i].node_count());
        file << elems[i].node_count();
        for(int j = 0; j < elems[i].node_count(); j++)
            file << ' ' << elems[i].get_vertex_index(j);
        file << '\n';
    }
    // Write out the element types
    file << "\nCELL_TYPES " << pd.num_elements() << '\n';
    for(size_t i = 0; i < pd.num_elements(); i++)
    {
        const Mesquite::VtkTypeInfo *info = Mesquite::VtkTypeInfo::find_type(
                elems[i].get_element_type(),
                elems[i].node_count(),
                err);
        MSQ_ERRRTN(err);
        file << info->vtkType << '\n';
    }

    if(write_codes)
    {
        // Write out normals at nodes.
        file << "POINT_DATA " << pd.num_nodes()
                << "\nSCALARS codes float\nLOOKUP_TABLE default\n";
        for(size_t i = 0; i < pd.num_vertices(); ++i)
        {
            unsigned code;
            this->get_codes(&(pd.get_vertex_handles_array()[i]), &code, 1);
            file << code << '\n';
        }
    }
    else
    {
        // Write out normals at nodes.
        file << "\nPOINT_DATA " << pd.num_vertices()
                << "\nNORMALS node_normal float\n";
        for(size_t i = 0; i < pd.num_vertices(); ++i)
        {
            Vector3D N = vert[i];
            this->normal_at(pd.get_vertex_handles_array()[i], N);
            file << N.x() << ' ' << N.y() << ' ' << N.z() << '\n';
        }

        file << "\nSCALARS codes float\nLOOKUP_TABLE default\n";
        for(size_t i = 0; i < pd.num_vertices(); ++i)
        {
            unsigned code;
            this->get_codes(&(pd.get_vertex_handles_array()[i]), &code, 1);
            file << code << '\n';
        }

        // Write out normals at faces.
        file << "\nCELL_DATA " << pd.num_elements()
                << "\nNORMALS face_normal float\n";
        for(size_t i = 0; i < pd.num_elements(); ++i)
        {
            Vector3D N;
            elems[i].get_centroid(N, pd, err);
            this->normal_at(pd.get_element_handles_array()[i], N);
            file << N.x() << ' ' << N.y() << ' ' << N.z() << '\n';
        }

        // Close the file
        file << '\n';
        file.close();

    }
}


// *****************************************************************************

static void mergeAdjacencyNp1(const std::vector< std::list<int> > &adj1,
        const std::vector< std::list<int> > &adjN,
        std::vector< std::list<int> > &adjNp1)
{
    size_t NN = adj1.size();
    for(size_t n = 0; n < NN; ++n)
    {
        adjNp1[n].clear();
        std::list<int>::const_iterator ii;
        for(ii = adj1[n].begin(); ii != adj1[n].end(); ++ii)
            adjNp1[n].insert(adjNp1[n].begin(),
                adjN[*ii].begin(), adjN[*ii].end());
        adjNp1[n].sort();
        adjNp1[n].unique();
    }
}

void SurfAlign_grid3d::createAdjacency()
{
    size_t NN = surf_nodes.size();

    AdjacentNodes.resize(NN);
    AdjacentNodes2.resize(NN);
    AdjacentNodes3.resize(NN);

    // make the direct adjacency lists
    std::vector<SurfFace>::const_iterator face;
    for(face = surf_faces.begin(); face != surf_faces.end(); ++face)
    {
        int face_surf_nodes[3] = {face->surf_nodes(0, nodes_inv),
            face->surf_nodes(1, nodes_inv), face->surf_nodes(2, nodes_inv)};
        for(int i = 0; i < 3; i++)
            for(int j = 0; j < 3; j++)
                AdjacentNodes[face_surf_nodes[i]].push_back(face_surf_nodes[j]);
    }
    for(int n = 0; n < NN; ++n)
    {
        AdjacentNodes[n].sort();
        AdjacentNodes[n].unique();
    }

    mergeAdjacencyNp1(AdjacentNodes, AdjacentNodes, AdjacentNodes2);
    mergeAdjacencyNp1(AdjacentNodes, AdjacentNodes2, AdjacentNodes3);
}


